﻿using SoftFloat;
using System;
using System.Runtime.CompilerServices;
using UnityEngine;

namespace BEPUutilities
{
    public partial struct NQuaternion
    {
        public static NVector3 operator *(NQuaternion rotation, NVector3 point)
        {
            sfloat num = rotation.X * 2f;
            sfloat num2 = rotation.Y * 2f;
            sfloat num3 = rotation.Z * 2f;
            sfloat num4 = rotation.X * num;
            sfloat num5 = rotation.Y * num2;
            sfloat num6 = rotation.Z * num3;
            sfloat num7 = rotation.X * num2;
            sfloat num8 = rotation.X * num3;
            sfloat num9 = rotation.Y * num3;
            sfloat num10 = rotation.W * num;
            sfloat num11 = rotation.W * num2;
            sfloat num12 = rotation.W * num3;
            NVector3 result = default;
            result.X = (1f - (num5 + num6)) * point.X + (num7 - num12) * point.Y + (num8 + num11) * point.Z;
            result.Y = (num7 + num12) * point.X + (1f - (num4 + num6)) * point.Y + (num9 - num10) * point.Z;
            result.Z = (num8 - num11) * point.X + (num9 + num10) * point.Y + (1f - (num4 + num5)) * point.Z;
            return result;
        }

        public static NQuaternion Lerp(NQuaternion quaternion1, NQuaternion quaternion2, sfloat amount)
        {
            sfloat num1 = amount;
            sfloat num2 = 1f - num1;
            var quaternion = new NQuaternion();
            if (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y +
                quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W >= 0.0)
            {
                quaternion.X = num2 * quaternion1.X + num1 * quaternion2.X;
                quaternion.Y = num2 * quaternion1.Y + num1 * quaternion2.Y;
                quaternion.Z = num2 * quaternion1.Z + num1 * quaternion2.Z;
                quaternion.W = num2 * quaternion1.W + num1 * quaternion2.W;
            }
            else
            {
                quaternion.X = num2 * quaternion1.X - num1 * quaternion2.X;
                quaternion.Y = num2 * quaternion1.Y - num1 * quaternion2.Y;
                quaternion.Z = num2 * quaternion1.Z - num1 * quaternion2.Z;
                quaternion.W = num2 * quaternion1.W - num1 * quaternion2.W;
            }

            sfloat num3 = 1f / libm.Sqrt(quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z + quaternion.W * quaternion.W);
            quaternion.X *= num3;
            quaternion.Y *= num3;
            quaternion.Z *= num3;
            quaternion.W *= num3;
            return quaternion;
        }

        public static NQuaternion LookRotation(NVector3 forward, NVector3 upwards)
        {
            forward = NVector3.Normalize(forward);
            var right = NVector3.Normalize(NVector3.Cross(upwards, forward));
            upwards = NVector3.Cross(forward, right);

            var m00 = right.X;
            var m01 = upwards.X;
            var m02 = forward.X;
            var m10 = right.Y;
            var m11 = upwards.Y;
            var m12 = forward.Y;
            var m20 = right.Z;
            var m21 = upwards.Z;
            var m22 = forward.Z;

            var trace = m00 + m11 + m22;
            var q = new NQuaternion();

            if (trace > 0f)
            {
                var num = libm.Sqrt(1f + trace);
                q.W = num * 0.5f;
                num = 0.5f / num;
                q.X = (m21 - m12) * num;
                q.Y = (m02 - m20) * num;
                q.Z = (m10 - m01) * num;
                return q;
            }

            if (m00 >= m11 && m00 >= m22)
            {
                sfloat num = libm.Sqrt(1f + m00 - m11 - m22);
                sfloat num4 = 0.5f / num;
                q.X = 0.5f * num;
                q.Y = (m01 + m10) * num4;
                q.Z = (m02 + m20) * num4;
                q.W = (m12 - m21) * num4;
                return q;
            }

            if (m11 > m22)
            {
                sfloat num6 = libm.Sqrt(1f + m11 - m00 - m22);
                sfloat num3 = 0.5f / num6;
                q.X = (m10 + m01) * num3;
                q.Y = 0.5f * num6;
                q.Z = (m21 + m12) * num3;
                q.W = (m02 - m20) * num3;
                return q;
            }

            var num5 = libm.Sqrt(1f + m22 - m00 - m11);
            var num2 = 0.5f / num5;
            q.X = (m20 + m02) * num2;
            q.Y = (m21 + m12) * num2;
            q.Z = 0.5f * num5;
            q.W = (m01 - m10) * num2;
            return q;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetAxisAngleFromQuaternion(ref NQuaternion q, out NVector3 axis, out sfloat angle)
        {
#if !WINDOWS
            axis = new NVector3();
#endif
            sfloat qw = q.W;
            if (qw > (sfloat)0)
            {
                axis.X = q.X;
                axis.Y = q.Y;
                axis.Z = q.Z;
            }
            else
            {
                axis.X = -q.X;
                axis.Y = -q.Y;
                axis.Z = -q.Z;
                qw = -qw;
            }

            sfloat lengthSquared = axis.LengthSquared();
            if (lengthSquared > (sfloat)1e-14f)
            {
                NVector3.Divide(ref axis, libm.Sqrt(lengthSquared), out axis);
                angle = (sfloat)2 * libm.Acos(MathHelper.Clamp(qw, (sfloat)(-1), (sfloat)1));
            }
            else
            {
                axis = Toolbox.UpVector;
                angle = (sfloat)0;
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Subtract(in NQuaternion quaternion1, in NQuaternion quaternion2, out NQuaternion result)
        {
            result.X = quaternion1.X - quaternion2.X;
            result.Y = quaternion1.Y - quaternion2.Y;
            result.Z = quaternion1.Z - quaternion2.Z;
            result.W = quaternion1.W - quaternion2.W;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static NQuaternion operator -(in NQuaternion value1, in NQuaternion value2)
        {
            Subtract(value1, value2, out NQuaternion result);
            return result;
        }

#if UNITY
        public static bool operator ==(UnityEngine.Quaternion lhs, NQuaternion rhs)
        {
            return ((NQuaternion)lhs - rhs).Length() < 9.9999994E-11f;
        }

        public static bool operator !=(UnityEngine.Quaternion lhs, NQuaternion rhs)
        {
            return !(lhs == rhs);
        }

        public static implicit operator UnityEngine.Quaternion(NQuaternion v)
        {
            return new UnityEngine.Quaternion(v.X, v.Y, v.Z, v.W);
        }

        public static implicit operator NQuaternion(UnityEngine.Quaternion v)
        {
            return new NQuaternion(v.x, v.y, v.z, v.w);
        }

        public static bool operator ==(UnityEngine.Vector4 lhs, NQuaternion rhs)
        {
            return ((NQuaternion)lhs - rhs).Length() < (sfloat)9.9999994E-11f;
        }

        public static bool operator !=(UnityEngine.Vector4 lhs, NQuaternion rhs)
        {
            return !(lhs == rhs);
        }

        public static implicit operator UnityEngine.Vector4(NQuaternion v)
        {
            return new UnityEngine.Vector4(v.X, v.Y, v.Z, v.W);
        }

        public static implicit operator NQuaternion(UnityEngine.Vector4 v)
        {
            return new NQuaternion(v.x, v.y, v.z, v.w);
        }
#endif
    }
}